"""
Author: Morphlng
Date: 2024-03-19 13:51:41
LastEditTime: 2024-03-19 13:51:42
LastEditors: Morphlng
Description: Test for location
FilePath: /DrivingGym/tests/common/geom/location_test.py
"""

import math

import numpy as np

from driving_gym.simulation.common.geom import Location


def is_close(a, b):
    return math.isclose(a + 1, b + 1)


class TestLocation:

    def test_initialization(self):
        loc = Location(1, 2, 3)
        assert (
            loc.x == 1 and loc.y == 2 and loc.z == 3
        ), "Initialization of Location failed."

    def test_from_gps(self):
        latitude, longitude, altitude = 0.0, 0.0, 100.0
        location = Location.from_gps(latitude, longitude, altitude)
        assert isinstance(
            location, Location
        ), "Location.from_gps failed to return a Location instance."

        assert (
            is_close(location.x, 0.0)
            and is_close(location.y, 0.0)
            and is_close(location.z, altitude)
        ), "from_gps method calculation error."
